#include "can_app.h"


osThreadId_t can_TaskHandle;
const osThreadAttr_t can_Task_attributes = {
  .name = "can_Task",
  .stack_size = CAN_TASK_STACK_SIZE * 4,
  .priority = (osPriority_t) CAN_STORAGE_TASK_PRIORITY,
};

void can_Task(void *argument)
{
  /* USER CODE BEGIN StartDefaultTask */
  /* Infinite loop */
    
	// 初始化DroneCAN
	canard_init();
	
  // Configure the filter
  if (CAN_Filter_Config() != 0) {
    Error_Handler();
  }
  
  // Start CAN
  if (HAL_CAN_Start(&hcan1) != HAL_OK) {
    Error_Handler();
  }
	
  // Enable the FIFO0 message suspension interrupt
  if (HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
    Error_Handler();
  }
	
	float received_adc_data[16];
	uint32_t last_send_time = 0;
	const uint32_t SEND_INTERVAL_MS = 1000; // 发送间隔1秒

	for(;;)
	{
			uint32_t current_time = HAL_GetTick();
			
			// 从队列接收ADC数据
			if (xQueueReceive(xQueue_Uart, received_adc_data, 0) == pdTRUE) {
				// 检查是否到达发送间隔
				if (current_time - last_send_time >= SEND_INTERVAL_MS) {
					printf("ADC data received, sending CircuitStatus...\n");
					// 发送CircuitStatus消息，使用ADC数据的第0个元素
					send_circuit_status(received_adc_data[0]);
					last_send_time = current_time;
				}
			}
				
			// 处理DroneCAN的TX队列 - 确保在每次循环中都处理
			const CanardCANFrame* txf = canardPeekTxQueue(&canard_inst);
			while (txf != NULL) {
				// 修正调试输出，显示原始和解析后的CAN ID
				uint32_t dronecan_id = txf->id & 0x1FFFFFFF;
				printf("Sending frame - Raw CAN ID: 0x%08X, DroneCAN ID: 0x%08X\n", 
					txf->id, dronecan_id);
				
				// 发送CAN帧
				if (CAN_Send_Msg(txf->id, (uint8_t*)txf->data, txf->data_len, 1) == 0) {
					printf("CAN frame sent successfully\n");
					// 发送成功，从队列移除
					canardPopTxQueue(&canard_inst);
				} else {
					printf("CAN frame send failed\n");
					// 发送失败，跳出循环
					break;
				}
				txf = canardPeekTxQueue(&canard_inst);
			}
			
			// 清理过时的传输
			canardCleanupStaleTransfers(&canard_inst, HAL_GetTick() * 1000);
			
			osDelay(10);
		}
}
